﻿namespace TrueSync.Physics2D
{
    public class CircleShape:Shape
    {
        internal FP _radius;
        internal FP _2radius;

        public override ShapeType ShapeType
        {
            get
            {
                return ShapeType.Circle;
            }
        }

        public CircleShape(FP radius)
            : base()
        {
            Radius = radius; // The Radius property cache 2radius and calls ComputeProperties(). So no need to call ComputeProperties() here.
        }

        public CircleShape():base()
        {
            _radius = 0;
        }

        public FP Radius
        {
            get { return _radius; }
            set
            {
                if (value < 0)
                    return;
                _radius = value;
                _2radius = _radius * _radius;
            }
        }

        public override bool TestPoint(TSTransform transform, TSVector2 point)
        {
            TSVector2 center = transform.p; 
            TSVector2 d = point - center;
            return d.LengthSquared() <= _2radius;
        }

        public override bool RayCast(out RayCastOutput output, RayCastInput input, TSTransform transform)
        {
            output = new RayCastOutput();

            TSVector2 position = transform.p;
            TSVector2 s = input.Point1 - position;
            FP b = TSVector2.Dot(s, s) - _2radius;

            // Solve quadratic equation.
            TSVector2 r = input.Point2 - input.Point1;
            FP c = TSVector2.Dot(s, r);
            FP rr = TSVector2.Dot(r, r);
            FP sigma = c * c - rr * b;

            // Check for negative discriminant and short segment.
            if (sigma < 0.0f || rr < Settings.Epsilon)
            {
                return false;
            }

            // Find the point of intersection of the line with the circle.
            FP a = -(c + FP.Sqrt(sigma));

            // Is the intersection point on the segment?
            if (FP.Zero <= a && a <= input.MaxFraction * rr)
            {
                a /= rr;
                output.Fraction = a;

                //TODO: Check results here
                output.Normal = s + a * r;
                output.Normal.Normalize();
                return true;
            }

            return false;
        }

        public override void ComputeAABB(out AABB aabb, TSTransform transform)
        {
            TSVector2 p = transform.p;
            aabb = new AABB(new TSVector2(p.x - Radius, p.y - Radius), new TSVector2(p.x + Radius, p.y + Radius));
        }

        public override bool CompareTo(Shape shape)
        {
            if(shape is CircleShape)
            {
                CircleShape other = (CircleShape)shape;
                if (other.Radius == Radius)
                    return true;
            }

            return false;
        }

        public override Shape Clone()
        {
            CircleShape shape = new CircleShape(_radius);
            return shape;
        }
    }
}

